// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectAgentMessageProcessorV2.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Runtime.Cameras
{
    using System;
    using System.Drawing;
    using System.IO; 
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.Cameras.KinectV2;

    /// <summary>
    /// Kinect message processor for kinect V2. 
    /// Constructs the legacy kinect message from the V2 depth, IR or color messages
    /// When averaging is enabled (averaging count > 1) a new message when sufficient messages
    /// have been received on either channel
    /// When there is no averaging (averaging count == 1 or less). Agent waits until all messages have been
    /// received
    /// </summary>
    [DataContract]
    public class KinectAgentMessageProcessorV2 : ConsumerProducerAgent<DepthCameraAgentMessage<DepthParameters>, DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>>,
                                                ISubscriptionReceiver<IRCameraAgentMessage<IRParameters>>,
                                                ISubscriptionReceiver<ColorCameraAgentMessage<ColorParameters>>
    {
        /// <summary>
        /// Cache the depth message
        /// </summary>
        private DepthCameraAgentMessage<DepthParameters> depthMessage;

        /// <summary>
        /// Cache of the IR message
        /// </summary>
        private IRCameraAgentMessage<IRParameters> irMessage;

        /// <summary>
        /// Cached color message
        /// </summary>
        private ColorCameraAgentMessage<ColorParameters> colorMessage;

        /// <summary>
        /// Target number of frames to average before publishing
        /// </summary>
        [DataMember(Name = "AveragingCount")]
        private int averagingCount;

        /// <summary>
        /// The kinect V2 processor
        /// </summary>
        private KinectV2Processor kinectV2Processor;

        /// <summary>
        /// Tracks if an IR message has been seen since agent started
        /// </summary>
        private bool isIRreceived;

        /// <summary>
        /// Tracks if a depth message has been seen since agent started
        /// </summary>
        private bool isDepthReceived;

        /// <summary>
        /// Tracks if a color message has been seen since agent started
        /// </summary>
        private bool isColorReceived;

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectAgentMessageProcessorV2"/> class. 
        /// </summary>
        /// <param name="name">Processor name</param>
        /// <param name="producer">Kinect V2 camera should be producer</param>
        /// <param name="averagingCount">Number of raw frames to average per publish</param>
        public KinectAgentMessageProcessorV2(string name, IProducer<DepthCameraAgentMessage<DepthParameters>> producer, int averagingCount)
            : base(name, producer.Name)
        {
            this.averagingCount = averagingCount;
        }

        /// <summary>
        /// Initializes the agent instance
        /// </summary>
        /// <param name="locator">A locator that the agent can use to discover other agents</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);
            this.ProducerAgent.Subscribe<IRCameraAgentMessage<IRParameters>>(this);
            this.ProducerAgent.Subscribe<ColorCameraAgentMessage<ColorParameters>>(this);
        }

        /// <summary>
        /// Called when the agent is about to start running
        /// </summary>
        public override void OnActivated()
        {
            base.OnActivated();
            this.isDepthReceived = false;
            this.isIRreceived = false;
            this.isColorReceived = false;
            this.kinectV2Processor = new KinectV2Processor(this.averagingCount);
        }

        /// <summary>
        /// Receive the IR message
        /// </summary>
        /// <param name="irMessage">received IR message from kinect camera V2</param>
        public void Receive(IRCameraAgentMessage<IRParameters> irMessage)
        {
            this.irMessage = irMessage;
            this.isIRreceived = true;
            this.AccumulateIRFrame(irMessage);
        }

        /// <summary>
        /// Receive the color message
        /// </summary>
        /// <param name="colorMessage">received color message from kinect camera V2</param>
        public void Receive(ColorCameraAgentMessage<ColorParameters> colorMessage)
        {
            this.isColorReceived = true;
            this.colorMessage = colorMessage;
        }

        /// <summary>
        /// Receive the depth message
        /// </summary>
        /// <param name="depthMessage">Received depth image</param>
        public override void Receive(DepthCameraAgentMessage<DepthParameters> depthMessage)
        {
            this.isDepthReceived = true;
            this.depthMessage = depthMessage;
            this.AccumulateDepthFrame(depthMessage);
        }

        /// <summary>
        /// Post a composite kinect agent message from raw message frames
        /// </summary>
        private void PostCompositeMessageNoAverage()
        {
            // Publish a messsage only if all messages which have been seen since the agent started
            // have been received
            if ((false == this.isDepthReceived || null != this.depthMessage) &&
                (false == this.isIRreceived || null != this.irMessage) &&
                (false == this.isColorReceived || null != this.colorMessage))
            {
                DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters> msg = new DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>(
                    AgentMessage.GetCurrentTime(),
                    this.depthMessage,
                    this.irMessage,
                    this.colorMessage);

                this.Publisher.Post(msg);
                this.depthMessage = null;
                this.irMessage = null;
                this.colorMessage = null;
            }
        }

        /// <summary>
        /// Post a composite kinect agent message from averaged frames
        /// </summary>
        private void PostCompositeMessage()
        {
            long origTime = this.kinectV2Processor.AccumulateStartTime;
            DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters> msg = new DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>(
                    origTime,
                this.depthMessage == null ? null : new DepthCameraAgentMessage<DepthParameters>(origTime, this.kinectV2Processor.GetDepthFrameCopy(), this.depthMessage.Parameters),
                this.irMessage == null ? null : new IRCameraAgentMessage<IRParameters>(origTime, this.kinectV2Processor.GetIRFrameCopy(), this.irMessage.Parameters),
                this.colorMessage);

            this.Publisher.Post(msg);
            this.kinectV2Processor.ResetAveragingAccumulators();
        }

        /// <summary>
        /// Add current depth message to running depth average
        /// </summary>
        /// <param name="msg">Incoming message to add to average</param>
        private void AccumulateDepthFrame(DepthCameraAgentMessage<DepthParameters> msg)
        {
            if (this.averagingCount <= 1)
            {
                this.PostCompositeMessageNoAverage();
            }
            else if (true == this.kinectV2Processor.AccumulateDepthFrame(msg.DepthFrame, (short)msg.Parameters.TooNearDepth, (short)msg.Parameters.TooFarDepth, msg.OriginatingTime))
            {
                this.PostCompositeMessage();
            }
        }

        /// <summary>
        /// Add current depth message to running depth average
        /// </summary>
        /// <param name="msg">Incoming message to add to average</param>
        private void AccumulateIRFrame(IRCameraAgentMessage<IRParameters> msg)
        {
            if (this.averagingCount <= 1)
            {
                this.PostCompositeMessageNoAverage();
            }
            else if (true == this.kinectV2Processor.AccumulateIRFrame(msg.ImageFrame, msg.OriginatingTime))
            {
                this.PostCompositeMessage();
            }
        }
    }
}
